/***************************************************************************************************************************
 * Author: guanty18
 * * Update Time: 2021.8.11
***************************************************************************************************************************/
#include <math.h>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <sensor_msgs/Range.h>
#include <uav/servo.h>
#include <uav/position.h>

//*******************************全局变量*******************************

//参数
float takeoff_height;
float payload_height;
float camera_x;
float camera_y;
float servo_x[3];
float servo_y[3];
float error[3];


bool turn_offboard = false;
bool is_land=false;
int command_num=-1;
int find_num=0;

//舵机操控信息
uav::servo servo_info;
int servo_id=0;

//无人机飞行地图目标位置
geometry_msgs::PoseStamped pose;//无人机飞行实际目标位置
geometry_msgs::PoseStamped real_pose;
//pose.pose.position.x = 0;
//pose.pose.position.y = 0;
//pose.pose.position.z = 0;



//无人机状态变量及其订阅回调
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}


float tfmini_z;
void tfmini_cb(const sensor_msgs::Range::ConstPtr& msg)
{
    tfmini_z = msg->range;
}

//无人机OFFBOARD模式
void rc_cb(const mavros_msgs::RCIn::ConstPtr& rcmsg)
{
	if (rcmsg->channels[7] > 1800) 
    {
		turn_offboard = true;
	} 
    else 
    {
		turn_offboard = false;
	}
}

//无人机实际位置变量及其订阅回调
int clock_flag=0;
bool is_arrived = false;
uav::position position_now;
void pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    double dx = msg->pose.position.x - pose.pose.position.x;
	double dy = msg->pose.position.y - pose.pose.position.y;
    double dz = msg->pose.position.z - pose.pose.position.z;
	if (abs(dx) < error[0] && abs(dy) < error[1] && abs(dz) < error[2]) {
        clock_flag++;
		if (!is_arrived && clock_flag>=20)
			is_arrived = true;
    }

    position_now.x = msg->pose.position.x;
    position_now.y = msg->pose.position.y;
    position_now.z = msg->pose.position.z;
}

//无人机摄像头目标变量及其订阅回调
float pos_error;
uav::position position_target;
void target_cb(const uav::position::ConstPtr& msg){
    
	float view_L = position_now.z * 0.577;
    float x_error = view_L*(msg->x) + camera_x + servo_x[servo_id];
    float y_error = view_L*(msg->y) + camera_y + servo_y[servo_id];

    pos_error=x_error*x_error+y_error*y_error;

    position_target.x=position_now.x-y_error;
    position_target.y=position_now.y-x_error;
    position_target.z=position_now.z;
}

//激光雷达最近点-障碍物
uav::position position_barrier_tmp;
uav::position position_barrier;
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg){
    float distance_min = msg->ranges[0];
    float angle = msg->angle_min;
    for(int i = 0; i < 360; i++) {
        if(msg->ranges[i] < distance_min){
            angle = (msg->angle_min + msg->angle_increment * i);
            distance_min = msg->ranges[i];
        }
    }
    position_barrier_tmp.x=distance_min*cos(angle);
    position_barrier_tmp.y=distance_min*sin(angle);
}



//函数封装
void fly_to_pose(ros::Publisher local_pos_pub, ros::Rate rate)
{
    while(ros::ok() && !is_arrived && turn_offboard)
    {   
		local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
        ROS_INFO("point: %d, \ntarget.xyz: %0.5f/%0.5f/%0.5f, \npose.xyz: %0.5f/%0.5f/%0.5f", command_num,
		        pose.pose.position.x, pose.pose.position.y, pose.pose.position.z,
				position_now.x, position_now.y, position_now.z);

    }

    is_arrived=false;
    clock_flag=0;
}


//*******************************主函数********************************
int main(int argc, char **argv)
{
    ros::init(argc, argv, "square");
    ros::NodeHandle nh;


    
    //订阅和发布消息
    ros::Subscriber rc_sub = nh.subscribe<mavros_msgs::RCIn>
            ("/mavros/rc/in", 50, rc_cb);
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("/mavros/state", 50, state_cb);
    ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>
            ("/mavros/local_position/pose", 50, pos_cb);
    ros::Subscriber laser_sub = nh.subscribe<sensor_msgs::LaserScan>
            ("/scan", 50, scanCallback);
    ros::Subscriber target_sub = nh.subscribe<uav::position>
            ("/uav/target", 50, target_cb);
	ros::Subscriber tfmini_sub = nh.subscribe<sensor_msgs::Range>
            ("/mavros/distance_sensor/hrlv_ez4_pub", 50, tfmini_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("/mavros/setpoint_position/local", 20);
    ros::Publisher cmd_vel_pub = nh.advertise<geometry_msgs::Twist>
            ("/mavros/setpoint_velocity/cmd_vel_unstamped", 20);
    ros::Publisher servo_pub = nh.advertise<uav::servo>
            ("/uav/servo", 20);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("/mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("/mavros/set_mode");

    // 参数
    float Ax = 1.0;
    float Ay = 0.0;
    float Bx = 0.0;
    float By = -1.0;
    float Cx = 1.0;
    float Cy = -1.0;   
    nh.param<float>("takeoff_height", takeoff_height, 1.0);
    nh.param<float>("payload_height", payload_height, 0.2);
    nh.param<float>("errorx", error[0], 0.1);
    nh.param<float>("errory", error[1], 0.1);
    nh.param<float>("errorz", error[2], 0.05);
    nh.param<float>("camera_x", camera_x, 0.0);
    nh.param<float>("camera_y", camera_y, 0.0);
    nh.param<float>("servo1x", servo_x[0], 0.0);
    nh.param<float>("servo1y", servo_y[0], 0.0);    
    nh.param<float>("servo2x", servo_x[1], 0.0);
    nh.param<float>("servo2y", servo_y[1], 0.0);    
    nh.param<float>("servo3x", servo_x[2], 0.0);
    nh.param<float>("servo3y", servo_y[2], 0.0);




	ros::Rate rate(20);
    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::Twist cmd_vel;
    cmd_vel.linear.x = 0.1;
    cmd_vel.linear.y = 0.1;
    cmd_vel.linear.z = 0.1;

    cmd_vel.angular.x = 0;
    cmd_vel.angular.y = 0;
    cmd_vel.angular.z = 0;
    cmd_vel_pub.publish(cmd_vel);

    

	mavros_msgs::SetMode set_mode;
	mavros_msgs::CommandBool arm_cmd;
	set_mode.request.custom_mode = "OFFBOARD";
	arm_cmd.request.value = true;
	
	pose.pose.position.x = 0;
    pose.pose.position.y = 0;
	pose.pose.position.z = 0;
    ros::Time last_request = ros::Time::now();

    ROS_INFO("success");

    while(ros::ok()){
		if (turn_offboard && !is_land)
        {   
            while( !current_state.armed && ((ros::Time::now() - last_request) > ros::Duration(0.2)))
            {
                if( arming_client.call(arm_cmd) && arm_cmd.response.success)
                {
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }

			while( current_state.mode != "OFFBOARD" && ((ros::Time::now() - last_request) > ros::Duration(0.05)))
            {   
                set_mode.request.custom_mode = "OFFBOARD";
                local_pos_pub.publish(pose);
				if( set_mode_client.call(set_mode) && set_mode.response.mode_sent)
                {
					ROS_INFO("Offboard enabled");
				}
                last_request = ros::Time::now();
            }
            

        }

        if( current_state.armed )
        {

        switch (command_num) 
        {
			case -1:
				if (current_state.mode == "OFFBOARD" && current_state.armed){
					local_pos_pub.publish(pose);
					command_num++;
				}
				break;
			case 0:
                //takeoff
                pose.pose.position.x = 0.0;
                pose.pose.position.y = 0.0;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                std::cout << "take off!"<<std::endl;
                command_num++;
                break;

            case 1:
                // from O to A (nearly)
                pose.pose.position.x = Ax;
                pose.pose.position.y = Ay;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                std::cout << "arrived at A"<<std::endl;
                command_num++;
                break;




            case 2:
                // from barrier1 to B
                pose.pose.position.x = Bx;
                pose.pose.position.y = By;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                std::cout << "arrived at B"<<std::endl;
                command_num++;
                break;


            
            case 3:
                // from barrier2 to C
                pose.pose.position.x = Cx;
                pose.pose.position.y = Cy;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                std::cout << "arrived at C"<<std::endl;
                command_num++;
                break;



            case 4:    


                pose.pose.position.x = 0.0;
                pose.pose.position.y = 0.0;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
            
                command_num=1;
                break;

			
			case 15:
                set_mode.request.custom_mode = "AUTO.LAND";
                last_request = ros::Time::now();
                while( current_state.mode != "AUTO.LAND" && ((ros::Time::now() - last_request) > ros::Duration(2.0)))
                {
                     if( set_mode_client.call(set_mode) && set_mode.response.mode_sent)
                     {
                         std::cout << "AUTO.land"<<std::endl;
                     }
                }
                is_land=true;
               // 上锁
                last_request = ros::Time::now();
                arm_cmd.request.value = false;
                if( current_state.armed)
                {
                    if( arming_client.call(arm_cmd) &&arm_cmd.response.success)
                     {
                         std::cout << "Vehicle disarmed"<<std::endl;
                     }
                     last_request = ros::Time::now();
                 }
                break;
        }

        }
        

        ros::spinOnce();
        rate.sleep();
    }
    return 0;

}


